LowLevelParticleFilters

Types

We provide a number of filter types

• ParticleFilter: This filter is simple to use and assumes that both dynamics noise and measurement noise are additive.
• AuxiliaryParticleFilter: This filter is identical to ParticleFilter, but uses a slightly different proposal mechanism for new particles.
• AdvancedParticleFilter: This filter gives you more flexibility, at the expense of having to define a few more functions. More instructions on this type below.
• KalmanFilter. Is what you would expect. Has the same features as the particle filters, but is restricted to linear dynamics and gaussian noise.
• UnscentedKalmanFilter. Is also what you would expect. Has almost the same features as the Kalman filters, but handle nonlinear dynamics and measurement model, still requires an additive Gaussian noise model.
• ExtendedKalmanFilter. Runs a regular Kalman filter on linearized dynamics. Uses ForwardDiff.jl for linearization.

Functionality

• Filtering
• Smoothing (smooth)
• Parameter estimation using ML or PMMH (Particle Marginal Metropolis Hastings)

Particle filter

Defining a particle filter is straightforward, one must define the distribution of the noise df in the dynamics function, dynamics(x,u,t) and the noise distribution dg in the measurement function measurement(x,u,t). The distribution of the initial state d0 must also be provided. An example for a linear Gaussian system is given below.

using LowLevelParticleFilters, LinearAlgebra, StaticArrays, Distributions, Plots

Define problem

n = 2   # Dimension of state
m = 2   # Dimension of input
p = 2   # Dimension of measurements
N = 500 # Number of particles

const dg = MvNormal(p,1.0)          # Measurement noise Distribution
const df = MvNormal(n,1.0)          # Dynamics noise Distribution
const d0 = MvNormal(randn(n),2.0)   # Initial state Distribution

IsoNormal(
dim: 2
μ: [0.7923704311571288, 0.053478596528521966]
Σ: [4.0 0.0; 0.0 4.0]
)

Define random linear state-space system

Tr = randn(n,n)
const A = SMatrix{n,n}(Tr*diagm(0=>LinRange(0.5,0.95,n))/Tr)
const B = @SMatrix randn(n,m)
const C = @SMatrix randn(p,n)

2×2 StaticArrays.SMatrix{2, 2, Float64, 4} with indices SOneTo(2)×SOneTo(2):
  0.244706  -0.239882
 -0.68271   -0.106697

Next, we define the dynamics and measurement equations, they both take the signature (x,u,t) = (state, input, time)

dynamics(x,u,t) = A*x .+ B*u
measurement(x,u,t) = C*x
vecvec_to_mat(x) = copy(reduce(hcat, x)') # Helper function

vecvec_to_mat (generic function with 1 method)

We are now ready to define and use a filter

pf = ParticleFilter(N, dynamics, measurement, df, dg, d0)

ParticleFilter{PFstate{StaticArrays.SVector{2, Float64}, Float64}, typeof(Main.ex-lingauss.dynamics), typeof(Main.ex-lingauss.measurement), Distributions.ZeroMeanIsoNormal{Tuple{Base.OneTo{Int64}}}, Distributions.ZeroMeanIsoNormal{Tuple{Base.OneTo{Int64}}}, Distributions.IsoNormal, DataType, Random.MersenneTwister}
  state: PFstate{StaticArrays.SVector{2, Float64}, Float64}
  dynamics: dynamics (function of type typeof(Main.ex-lingauss.dynamics))
  measurement: measurement (function of type typeof(Main.ex-lingauss.measurement))
  dynamics_density: Distributions.ZeroMeanIsoNormal{Tuple{Base.OneTo{Int64}}}
  measurement_density: Distributions.ZeroMeanIsoNormal{Tuple{Base.OneTo{Int64}}}
  initial_density: Distributions.IsoNormal
  resample_threshold: Float64 0.1
  resampling_strategy: LowLevelParticleFilters.ResampleSystematic <: LowLevelParticleFilters.ResamplingStrategy
  rng: Random.MersenneTwister

With the filter in hand, we can simulate from its dynamics and query some properties

xs,u,y = simulate(pf,200,df) # We can simulate the model that the pf represents
pf(u[1], y[1])               # Perform one filtering step using input u and measurement y
particles(pf)                # Query the filter for particles, try weights(pf) or expweights(pf) as well
x̂ = weigthed_mean(pf)        # using the current state

2-element Vector{Float64}:
 -0.5065211725495822
 -2.017746113733322

If you want to perform filtering using vectors of inputs and measurements, try any of the functions

x,w,we,ll = forward_trajectory(pf, u, y) # Filter whole vectors of signals
x̂,ll = mean_trajectory(pf, u, y)
trajectorydensity(pf,x,w,u,y,xreal=xs)

If MonteCarloMeasurements.jl is loaded, you may transform the output particles to Matrix{MonteCarloMeasurements.Particles} with the layout T × n_states using Particles(x,we). Internally, the particles are then resampled such that they all have unit weight. This is conventient for making use of the plotting facilities of MonteCarloMeasurements.jl.

For a full usage example, see the benchmark section below or example_lineargaussian.jl

Smoothing

We also provide a particle smoother, based on forward filtering, backward simulation (FFBS)

N     = 2000 # Number of particles
T     = 200  # Number of time steps
M     = 100  # Number of smoothed backwards trajectories
pf    = ParticleFilter(N, dynamics, measurement, df, dg, d0)
du    = MvNormal(2,1)     # Control input distribution
x,u,y = simulate(pf,T,du) # Simulate trajectory using the model in the filter
tosvec(y) = reinterpret(SVector{length(y[1]),Float64}, reduce(hcat,y))[:] |> copy
x,u,y = tosvec.((x,u,y))

xb,ll = smooth(pf, M, u, y) # Sample smooting particles
xbm   = smoothed_mean(xb)   # Calculate the mean of smoothing trajectories
xbc   = smoothed_cov(xb)    # And covariance
xbt   = smoothed_trajs(xb)  # Get smoothing trajectories
xbs   = [diag(xbc) for xbc in xbc] |> vecvec_to_mat .|> sqrt
plot(xbm', ribbon=2xbs, lab="PF smooth")
plot!(vecvec_to_mat(x), l=:dash, lab="True")

We can plot the particles themselves as well

plot(vecvec_to_mat(x), l=(4,), layout=(2,1), show=false)
scatter!(xbt[1,:,:]', subplot=1, show=false, m=(1,:black, 0.5), lab="")
scatter!(xbt[2,:,:]', subplot=2, m=(1,:black, 0.5), lab="")